#ifndef LQR_TEST__AGV_MODEL_HPP_
#define LQR_TEST__AGV_MODEL_HPP_

#include <iostream>
#include <vector>
#include <math.h>
#include <stdint.h>
#include <string>

typedef struct RobotState
{
    double x, y, yaw, linear, angular;
};

typedef struct WayPoint
{
    int index;
    double x, y, yaw, k, linear, angular;
};

class AGVModule
{
private:
    double length_;
    double linear_, angular_;
    double x_, y_, yaw_;
    double dt_;
public:
    /**
     * @brief  Construct a new AGVModule object
     * @param  length: 轴距
     * @param  init_pose: 初始位置
     */
    AGVModule(double length, double freq, RobotState init_pose)
    {
        length_ = length;
        x_ = init_pose.x;
        y_ = init_pose.y;
        yaw_ = init_pose.yaw;
        dt_ = 1.0 / freq;
    }

    ~AGVModule()
    {

    }

    RobotState getPose()
    {
        RobotState cur;
        cur.x = x_;
        cur.y = y_;
        cur.yaw = yaw_;
        return cur;
    }

    void updateState(double linear, double angular)
    {
        x_ = x_ + linear * cos(yaw_) * dt_;
        y_ = y_ + linear * sin(yaw_) * dt_;
        yaw_ = yaw_ + angular * dt_;

        linear_ = linear;
        angular_ = angular;
    }


};
#endif